/*
    需求：编写参数服务端，设置并操作参数。
    步骤：
        1.包含头文件；
        2.初始化 ROS2 客户端；
        3.定义节点类；
            3-1.声明参数；
            3-2.查询参数；
            3-3.修改参数；
            3-4.删除参数。
        4.创建节点对象指针，调用参数操作函数，并传递给spin函数；
        5.释放资源。
*/

// 1.包含头文件；
#include"rclcpp/rclcpp.hpp"



// 3.定义节点类；
class MinimalParamServer:public rclcpp::Node
{
public:
    //对于rclcpp::Node，若想使用参数服务器，并且使用删除参数功能
    //为了避免安全隐患，必须在Node构造函数传入rclcpp::NodeOptions()，这里面允许删除参数
    //并利用rclcpp::NodeOptions().allow_undeclared_parameters(true)
    MinimalParamServer():Node("minimal_param_server",
    rclcpp::NodeOptions().allow_undeclared_parameters(true)){
        RCLCPP_INFO(this->get_logger(),"param_server start!");
    }
    //     3-1.声明参数；
    void declare_param(){
        RCLCPP_INFO(this->get_logger(),"add param!");
        // 声明参数并设置默认值
        this->declare_parameter("car_type","Tiger"); //string
        this->declare_parameter("height",1.50);
        this->declare_parameter("wheels",4);
        // 需要设置 rclcpp::NodeOptions().allow_undeclared_parameters(true),否则非法
        this->set_parameter(rclcpp::Parameter("undcl_test",100));
    }

    //     3-2.查询参数；
    void get_param(){
        RCLCPP_INFO(this->get_logger(),"check param!");
        // 获取指定参数
        rclcpp::Parameter car_type = this->get_parameter("car_type");
        RCLCPP_INFO(this->get_logger(),"car_type:%s",car_type.as_string().c_str());
        RCLCPP_INFO(this->get_logger(),"height:%.2f",this->get_parameter("height").as_double());
        RCLCPP_INFO(this->get_logger(),"wheels:%ld",this->get_parameter("wheels").as_int());
        RCLCPP_INFO(this->get_logger(),"undcl_test:%ld",this->get_parameter("undcl_test").as_int());
        // 判断包含，是否有这个参数
        RCLCPP_INFO(this->get_logger(),"包含car_type? %d",this->has_parameter("car_type"));
        RCLCPP_INFO(this->get_logger(),"包含car_typesxxxx? %d",this->has_parameter("car_typexxxx"));
        //获取一些参数
        auto params = this->get_parameters({"car_type","height","wheels"});
        for(auto &param :params)
        {
            RCLCPP_INFO(this->get_logger(),"name = %s,value = %s",param.get_name().c_str(),param.value_to_string().c_str());
        }
    } 

    //     3-3.修改参数；
    void update_param(){
        RCLCPP_INFO(this->get_logger(),"modify param!");
        this->set_parameter(rclcpp::Parameter("height",1.75)); //修改一个指定参数
        RCLCPP_INFO(this->get_logger(),"height:%.2f",this->get_parameter("height").as_double());
    }
    //     3-4.删除参数。
    void del_param(){
        // this->undeclare_parameter("car_type"); //删除一个指定参数
        // RCLCPP_INFO(this->get_logger(),"删除操作后，car_type还存在么? %d",this->has_parameter("car_type"));
        RCLCPP_INFO(this->get_logger(),"删除操作前,undcl_test存在么? %d",this->has_parameter("undcl_test"));
        this->undeclare_parameter("undcl_test");
        RCLCPP_INFO(this->get_logger(),"删除操作前,undcl_test存在么? %d",this->has_parameter("undcl_test"));
    }
    
private:

};

int main(int argc, char const *argv[])
{
    // 2.初始化 ROS2 客户端；
    rclcpp::init(argc,argv);

    // 4.创建节点对象指针，调用参数操作函数，并传递给spin函数；
    auto paramServer = std::make_shared<MinimalParamServer>();
    paramServer->declare_param();
    paramServer->get_param();
    paramServer->update_param();
    paramServer->del_param();
    rclcpp::spin(paramServer);

    // 5.释放资源。
    rclcpp::shutdown();
    return 0;
}


/*
    ...
    // 创建参数对象
    rclcpp::Parameter p1("car_name","Tiger"); //参数值为字符串类型
    rclcpp::Parameter p2("width",0.15); //参数值为浮点类型
    rclcpp::Parameter p3("wheels",2); //参数值为整型

    // 获取参数值并转换成相应的数据类型
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"car_name = %s", p1.as_string().c_str());
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"width = %.2f", p2.as_double());
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"wheels = %ld", p3.as_int());

    // 获取参数的键
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"p1 name = %s", p1.get_name().c_str());
    // 获取参数数据类型
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"p1 type_name = %s", p1.get_type_name().c_str());
    // 将参数值转换成字符串类型
    RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"p1 value_to_msg = %s", p1.value_to_string().c_str());
...
*/